#ifndef CMD_TASK_H
#define CMD_TASK_H

#include "struct_typedef.h"
#include "CAN_receive.h"
#include "pid.h"

//速度环 PID参数以及 PID最大输出，积分输出
#define DM_SPEED_PID_KP 1.2f 
#define DM_SPEED_PID_KI 0.0f  
#define DM_SPEED_PID_KD 0.2f
#define DM_SPEED_PID_MAX_OUT 10.0f 
#define DM_SPEED_PID_MAX_IOUT 0.0f
//位置环 PID参数以及 PID最大输出，积分输出
#define DM_POSITION_PID_KP 1.1f 
#define DM_POSITION_PID_KI 0.001f  
#define DM_POSITION_PID_KD 0.0f
#define DM_POSITION_PID_MAX_OUT 5.0f 
#define DM_POSITION_PID_MAX_IOUT 3.5f

//PS2拨杆VRX通道偏移量
#define PS2_VRX_DEFLECTION 32767 - 850

//PS2拨杆死区限制
#define PS2_VRX_DEALINE 2000

//PS2左右拨杆（VRX）转化成速度的比例
#define PS2_VRX_SEN 0.0005f

//PS2按键（SW）按下电机转动的角度
#define PS2_SW_MOVE_ANGLE 1.57f

typedef struct
{
    //原生数据
    uint16_t original_x;
    uint16_t original_y;
    uint8_t original_key;
    
    //上一次数据
    uint16_t last_x;
    uint16_t last_y;
    uint8_t last_key;
    
    //原生数据变形后得到的数据
    int16_t deflection_x;
    int16_t deflection_y;
    
    //实际使用的数据
    int16_t remove_x;
    int16_t remove_y;
} ps2_t;

typedef struct
{
    const dm_motor_measure_t *motor_measure;      //电机数据指针
    
    uint8_t ps2key_flag;
    uint8_t ready_move_flag;
    uint8_t vx_move_flag;
    uint8_t speed_mode;
    uint8_t position_mode;
    
    //fp32_UpdateMotorAngleSum();
    fp32 p_calc;
    fp32 last_p_calc;
    fp32 p_sum;
    
    fp32 p_set;
    fp32 v_set;
    fp32 v_send;
    fp32 given_current;
    
    pid_type_def speed_pid;
    pid_type_def position_pid;
} motor_t;

typedef struct
{
    ps2_t ps2;
    motor_t motor1;
} control_t;

extern void cmd_task(void);

#endif
